import os
import yaml
import pymap3d
import numpy as np

def parse_config(filename):
    """解析JSBSim配置文件(可能是一个YAML格式的文件)并将其转换为一个自定义类的对象

    Args:
        filename:这是传递给函数的配置文件的名称,不包括文件扩展名。例如,如果要解析名为my_config.yaml的配置文件,您将传递my_config作为filename参数。

    Returns:
        (EnvConfig): a custom class which parsing dict into object.
    """
    # 构建了配置文件的完整路径
    filepath = os.path.join(get_root_dir(), 'configs', f'{filename}.yaml')
    # 检查配置文件是否存在于指定路径,如果文件不存在,函数会引发一个断言错误,提供错误消息,说明配置文件不存在。
    # 如果配置文件存在,函数将打开文件,使用UTF-8编码,读取文件内容,并将其加载为YAML格式的数据。这里使用了PyYAML库的yaml.load函数来实现。
    assert os.path.exists(filepath), \
        f'config path {filepath} does not exist. Please pass in a string that represents the file path to the config yaml.'
    with open(filepath, 'r', encoding='utf-8') as f:
        config_data = yaml.load(f, Loader=yaml.FullLoader)
    # 最后,函数使用Python的type函数创建一个新的类,类的名称为EnvConfig,并将YAML加载的数据传递给类的构造函数。这样,函数返回的是一个具有配置文件数据的自定义对象。
    # 使用Python的type函数动态创建一个新的类,可以在type函数的参数中指定三个参数:
    # 新类的名称作为字符串。
    # 基类(父类)的元组,表示新类从哪些类派生而来。通常,可以使用object作为基类,表示新类是一个普通的Python类。
    # 类的属性和方法作为一个字典,其中键是属性或方法的名称,值是对应的属性值或方法。
    return type('EnvConfig', (object,), config_data)

def get_root_dir():
    return os.path.join(os.path.split(os.path.realpath(__file__))[0], '..')

def LLA2NEU(lon, lat, alt, lon0=120.0, lat0=60.0, alt0=0):
    """Convert from Geodetic Coordinate System to NEU Coordinate System.

    Args:
        lon, lat, alt (float): target geodetic lontitude(°), latitude(°), altitude(m)
        lon, lat, alt (float): observer geodetic lontitude(°), latitude(°), altitude(m); Default=`(120°E, 60°N, 0m)`

    Returns:
        (np.array): (North, East, Up), unit: m
    """
    n, e, d = pymap3d.geodetic2ned(lat, lon, alt, lat0, lon0, alt0)
    return np.array([n, e, -d])

def NEU2LLA(n, e, u, lon0=120.0, lat0=60.0, alt0=0):
    """Convert from NEU Coordinate System to Geodetic Coordinate System.

    Args:
        n, e, u (float): target relative position w.r.t. North, East, Down
        lon, lat, alt (float): observer geodetic lontitude(°), latitude(°), altitude(m); Default=`(120°E, 60°N, 0m)`

    Returns:
        (np.array): (lon, lat, alt), unit: °, °, m
    """
    lat, lon, h = pymap3d.ned2geodetic(n, e, -u, lat0, lon0, alt0)
    return np.array([lon, lat, h])


def get_AO_TA_R(ego_feature, enm_feature, return_side=False):
    """Get AO & TA angles and relative distance between two agent.

    Args:
        ego_feature & enemy_feature (tuple): (north, east, down, vn, ve, vd)

    Returns:
        (tuple): ego_AO, ego_TA, R
        
    这段代码定义了一个名为 get_AO_TA_R 的函数,该函数计算两个代理之间的角度和相对距离。函数接受两个代理的特征信息作为输入,并返回两个代理之间的角度以及它们之间的相对距离。以下是该函数的详细分析:

    函数参数:
        ego_feature 和 enm_feature:这两个参数是代理的特征信息的元组,包括北向、东向、下向速度以及相对速度的分量。特征信息包括 (north, east, down, vn, ve, vd)。
        return_side(可选参数):默认为 False,如果设置为 True,函数将返回额外的代理位置信息。

    变量初始化:
        提取了代理的位置和速度信息,包括位置 (ego_x, ego_y, ego_z) 和速度 (ego_vx, ego_vy, ego_vz),以及计算了代理的速度模长 ego_v。
        提取了敌方代理的位置和速度信息,包括位置 (enm_x, enm_y, enm_z) 和速度 (enm_vx, enm_vy, enm_vz),以及计算了敌方代理的速度模长 enm_v。
        计算了两个代理之间的相对位置矢量 delta_x, delta_y, delta_z,即敌方代理相对于自己代理的位置差。
        计算了两个代理之间的距离 R,即相对位置矢量的模长。

    计算 AO 和 TA 角度:
        计算了自己代理的 AO 角度(Angle Off):AO 角度是自己代理与敌方代理之间的角度,它表示了自己代理相对于敌方代理的方向偏离。
        计算了自己代理的 TA 角度(Track Angle):TA 角度是自己代理与敌方代理之间的角度,它表示了自己代理相对于敌方代理的追踪角度。

    返回值:
        如果 return_side 参数为 False,则函数返回计算得到的 AO 角度、TA 角度和相对距离 R,形成一个元组 (ego_AO, ego_TA, R)。
        如果 return_side 参数为 True,则额外返回一个代理的侧面标志 side_flag,该标志表示自己代理相对于敌方代理的位置关系(左侧、右侧或正前方)。

总结:get_AO_TA_R 函数用于计算两个代理之间的角度和相对距离,以帮助分析它们的相对位置和方向关系。这些角度可以在飞行任务等应用中用来确定代理之间的相对位置和方向,从而支持决策和控制。
    """
    ego_x, ego_y, ego_z, ego_vx, ego_vy, ego_vz = ego_feature
    ego_v = np.linalg.norm([ego_vx, ego_vy, ego_vz])
    enm_x, enm_y, enm_z, enm_vx, enm_vy, enm_vz = enm_feature
    enm_v = np.linalg.norm([enm_vx, enm_vy, enm_vz])
    delta_x, delta_y, delta_z = enm_x - ego_x, enm_y - ego_y, enm_z - ego_z
    R = np.linalg.norm([delta_x, delta_y, delta_z])

    proj_dist = delta_x * ego_vx + delta_y * ego_vy + delta_z * ego_vz
    ego_AO = np.arccos(np.clip(proj_dist / (R * ego_v + 1e-8), -1, 1))
    proj_dist = delta_x * enm_vx + delta_y * enm_vy + delta_z * enm_vz
    ego_TA = np.arccos(np.clip(proj_dist / (R * enm_v + 1e-8), -1, 1))

    if not return_side:
        return ego_AO, ego_TA, R
    else:
        side_flag = np.sign(np.cross([ego_vx, ego_vy], [delta_x, delta_y]))
        return ego_AO, ego_TA, R, side_flag


def get2d_AO_TA_R(ego_feature, enm_feature, return_side=False):
    ego_x, ego_y, ego_z, ego_vx, ego_vy, ego_vz = ego_feature
    ego_v = np.linalg.norm([ego_vx, ego_vy])
    enm_x, enm_y, enm_z, enm_vx, enm_vy, enm_vz = enm_feature
    enm_v = np.linalg.norm([enm_vx, enm_vy])
    delta_x, delta_y, delta_z = enm_x - ego_x, enm_y - ego_y, enm_z - ego_z
    R = np.linalg.norm([delta_x, delta_y])

    proj_dist = delta_x * ego_vx + delta_y * ego_vy
    ego_AO = np.arccos(np.clip(proj_dist / (R * ego_v + 1e-8), -1, 1))
    proj_dist = delta_x * enm_vx + delta_y * enm_vy
    ego_TA = np.arccos(np.clip(proj_dist / (R * enm_v + 1e-8), -1, 1))

    if not return_side:
        return ego_AO, ego_TA, R
    else:
        side_flag = np.sign(np.cross([ego_vx, ego_vy], [delta_x, delta_y]))
        return ego_AO, ego_TA, R, side_flag


def in_range_deg(angle):
    """ Given an angle in degrees, normalises in (-180, 180] """
    angle = angle % 360
    if angle > 180:
        angle -= 360
    return angle


def in_range_rad(angle):
    """ Given an angle in rads, normalises in (-pi, pi] """
    angle = angle % (2 * np.pi)
    if angle > np.pi:
        angle -= 2 * np.pi
    return angle

if __name__ == "__main__":
    config = parse_config('test')
    print(config)

    

